/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-07     86188       the first version
 */

#include <board.h>
#include <inc/uwb.h>
#include <rtdevice.h>
#include <rtthread.h>
#include <rtdbg.h>
#include <string.h>
#include <math.h>
#include <inc/filter.h>
#include <inc/motor_ctr.h>
#include <inc/timer.h>
#include <inc/gpio.h>


#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>



rt_device_t uwb_dev;
struct serial_configure u3_uwb_config = RT_SERIAL_CONFIG_UART3_UWB;
rt_thread_t uwb_rxtask;
rt_sem_t uwb_rxsem;


void createUwbDeviceAndTask(void)
{

    uwb_rxtask = rt_thread_create("uwb_task",uwbRxProTask,0,4096,10,10);
    if(uwb_rxtask != RT_NULL)
    {
        rt_thread_startup(uwb_rxtask);
        rt_kprintf("uwb create usart task secussee..\n");
    }

    uwb_rxsem = rt_sem_create("uwb_rxsem",0, RT_IPC_FLAG_PRIO);
    /*  create usart3 device   */


    uwb_dev = rt_device_find("uart3");
    if(uwb_dev != RT_NULL)
    {
        rt_kprintf("uwb create device secussee..\n");

        rt_device_open(uwb_dev, RT_DEVICE_FLAG_DMA_RX); //uart_init_rx   if you set dma recivce , use 'RT_DEVICE_FLAG_DMA_RX'
        rt_device_control(uwb_dev, RT_DEVICE_CTRL_CONFIG, (void *)&u3_uwb_config);
        rt_device_set_rx_indicate(uwb_dev, uwbCallback);

    }


}


rt_size_t u3_rx_len;

rt_err_t uwbCallback(rt_device_t dev, rt_size_t size)
{
    u3_rx_len = size;
    rt_sem_release(uwb_rxsem);

    return RT_EOK;
}
void uwbRxProTask(void *parameter)
{
    rt_uint8_t uwb_buf[256]={0};


    while(1)
    {
        rt_sem_take(uwb_rxsem, RT_WAITING_FOREVER);
        rt_device_read(uwb_dev, 0, uwb_buf, u3_rx_len);


    }
}

#define TAG1_ID 0x01

float uwbRawDatax=0;
float uwbRawDatay=0;

void uwb_decode(uint8_t *dat)
{
    if(dat[0] != 0xA5)
    {
        return;
    }
    else
    {
        if(dat[1] == TAG1_ID)
        {
            uwbRawDatax = (short)dat[2]<<8 | dat[3];
            if(uwbRawDatax > 60000)
                uwbRawDatax = 0;

            uwbRawDatay = (short)dat[4]<<8 | dat[5];
            if(uwbRawDatay > 60000)
                uwbRawDatay = 0;

        }


    }
}


///
uint8_t path_cnt = 0;
uint8_t path_num = 0;

float pathA[8][2] = {
 //x  , y
    //going path

         {POINT1_X, POINT1_Y},  //path0
         {POINT2_X, POINT2_Y},  //path1
         {POINT3_X, POINT3_Y},  //path2
         {POINTA_X, POINTA_Y},  //path3

    //return_path

         {POINT3_X, POINT3_Y},  //path4
         {POINT2_X, POINT2_Y},  //path5
         {POINT1_X, POINT1_Y},  //path6
         {START_POINT_X, START_POINT_Y},  //path7

};

float pathB[8][2] = {
 //x  , y
    //going path

         {POINT1_X, POINT1_Y},  //path0
         {POINT5_X, POINT5_Y},  //path1
         {POINT6_X, POINT6_Y},  //path2
         {POINTB_X, POINTB_Y},  //path3

    //return_path

         {POINT6_X, POINT6_Y},  //path4
         {POINT5_X, POINT5_Y},  //path5
         {POINT1_X, POINT1_Y},  //path6
         {START_POINT_X, START_POINT_Y},  //path7

};

float pathC[8][2] = {
 //x  , y
    //going path

         {POINT1_X, POINT1_Y},  //path0
         {POINT2_X, POINT2_Y},  //path1
         {POINT4_X, POINT4_Y},  //path2
         {POINTC_X, POINTC_Y},  //path3

    //return_path

         {POINT4_X, POINT4_Y},  //path4
         {POINT2_X, POINT2_Y},  //path5
         {POINT1_X, POINT1_Y},  //path6
         {START_POINT_X, START_POINT_Y},  //path7

};

float pathD[8][2] = {
 //x  , y
    //going path

         {POINT1_X, POINT1_Y},  //path0
         {POINT5_X, POINT5_Y},  //path1
         {POINT7_X, POINT7_Y},  //path2
         {POINTD_X, POINTD_Y},  //path3

    //return_path

         {POINT7_X, POINT7_Y},  //path4
         {POINT5_X, POINT5_Y},  //path5
         {POINT1_X, POINT1_Y},  //path6
         {START_POINT_X, START_POINT_Y},  //path7

};



float ret_rawx,ret_rawy;

uint8_t uwbDataFilter(uint8_t dt)
{
   static float last_x=0,last_y=0; ///上一次uwb位置

   static float uwb_vel_x=0,uwb_vel_y=0;///uwb速度

   static float encoder_vel_x=0,encoder_vel_y=0;//编码器速度

   static uint16_t error_cnt=0;

   static filter_t rawxFilter;
   static filter_t rawyFilter;


   static float rawx_buf[10],rawy_buf[10];

   static uint8_t buf_cnt=0;

   rawxFilter.alpha = 1.0f;
   rawyFilter.alpha = 1.0f;

   static uint8_t init_cnt=0; //初始化计数位

   static uint8_t move_delay_cnt=0;
   static uint8_t final_delay_cnt=0;
   static uint8_t error_delay_cnt=0;


   static uint8_t error_delay_flag=0;


   uwb_vel_x = (last_x - uwbRawDatax);
   uwb_vel_y = (last_y - uwbRawDatay);

   encoder_vel_x =  (((leftFrontMotor.speed + rightBackMotor.speed)/2 + (rightFrontMotor.speed + leftBackMotor.speed)/2) / 2);
   encoder_vel_y =  (((leftFrontMotor.speed + rightBackMotor.speed)/2 - (rightFrontMotor.speed + leftBackMotor.speed)/2) / 2);

//  printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",actualPos.x,actualPos.y,actualPos.z,uwb_vel_x,uwb_vel_y,encoder_vel_x,encoder_vel_y);


   if((fabs(uwb_vel_x) >= 15.0f) && (fabs(uwb_vel_y) >= 15.0f) && (init_cnt >= 2)) ///干扰数据
   {

       error_delay_flag = 1;///记标志位


       pidx_en = 0;//小车停止
       pidy_en = 0;
       pidz_en = 0;

       init_cnt = 11;
   }

   init_cnt++;

   if(error_delay_flag == 1) ///受到干扰 //1s不接收数据 小车停止
   {

       error_delay_cnt++;
       rt_kprintf("error:%d\n",error_delay_cnt);
//      BEEP_ON;
       if(error_delay_cnt >= 10) ///时间到 小车重新运动
       {
           error_delay_flag = 0;
           error_delay_cnt = 0;//计数值清零

           pidx_en = 1;
           pidy_en = 1;
           pidz_en = 1;

           last_x = uwbRawDatax;
           last_y = uwbRawDatay;


//          BEEP_OFF;
       }
   }
   else ///没有干扰 正常
   {

       ///1.传给上次的变量 用于下一次做加速度解算
       last_x = uwbRawDatax;
       last_y = uwbRawDatay;

       ///2.传给滤波器 直接输出当前位置
       rawxFilter.input = uwbRawDatax;   ///一阶滤波
       actualPos.x = LowPassFilter(&rawxFilter); //最终给控制器

       rawyFilter.input = uwbRawDatay;
       actualPos.y = LowPassFilter(&rawyFilter);


       ///3.传给排序buff 进行冒泡排序 取中值
       rawx_buf[buf_cnt] = uwbRawDatax;
       rawy_buf[buf_cnt] = uwbRawDatay;


       buf_cnt++;

       if(buf_cnt >= 10)
       {
           ret_rawx = bubbleSort(rawx_buf,10); //最终输出ret_rawx 滤波值
           ret_rawy = bubbleSort(rawy_buf,10);
           buf_cnt = 0;
       }



       ////4.期望坐标路径  path_cnt 初始值=0
       if(path_num == 'A')
       {
         desiredPos.x = pathA[path_cnt][0];
           desiredPos.y = pathA[path_cnt][1];
       }
       else if(path_num == 'B')
       {
           desiredPos.x = pathB[path_cnt][0];
           desiredPos.y = pathB[path_cnt][1];
       }
       else if(path_num == 'C')
       {
         desiredPos.x = pathC[path_cnt][0];
           desiredPos.y = pathC[path_cnt][1];
       }
       else if(path_num == 'D')
       {
           desiredPos.x = pathD[path_cnt][0];
           desiredPos.y = pathD[path_cnt][1];
       }




       ///5.判断小车到达哪个位置 如果是途径坐标 在附近即可判定到达  否者是最终坐标 坐标精确在±5cm左右

       if((path_cnt == 0) || (path_cnt == 1) || (path_cnt == 2) || (path_cnt == 4) || (path_cnt == 5) || (path_cnt == 6) ) ///途径坐标
       {

               if(

                   fabs(ret_rawx-desiredPos.x) <= 10.0f
                   && fabs(ret_rawy-desiredPos.y) <= 10.0f
                   &&fabs(uwb_vel_x) <= 2.8f
                   && fabs(uwb_vel_y) <= 2.8f
                 && fabs(encoder_vel_x) <= 2.8f
                   && fabs(encoder_vel_y) <= 2.8f
                   && fabs(desiredPos.z - actualPos.z) <= 0.3f
                 && move_delay_cnt == 0
               )
               {

                   path_cnt++;

                   BEEP_ON;
                   final_delay_cnt = 100;  //最终坐标延时10s
                   move_delay_cnt = 20; //途径坐标延时2s


//              pidReset(&leftFrontPid);
//              pidReset(&rightFrontPid);
//              pidReset(&rightBackPid);
//              pidReset(&leftBackPid);



               }


               if(move_delay_cnt)move_delay_cnt--;  ///定时时间到
               else    BEEP_OFF;
           }

           else if((path_cnt == 3) || (path_cnt == 7))///最终坐标
           {

           /// (1) 定时时间结束之前 位置到达 且各数值满足要求 进入停车状态
           ///（2）定时时间结束之后 位置在摆动 首先将车头回正 再重新pid微调 满足条件放宽  直至数值满足要求 进入停车状态
               if(
                   (fabs(ret_rawx-desiredPos.x) <= 3.5f
                   && fabs(ret_rawy-desiredPos.y) <= 3.5f
                   && fabs(uwb_vel_x) <= 2.1f
                   && fabs(uwb_vel_y) <= 2.1f
                   && fabs(encoder_vel_x) <= 2.1f
                   && fabs(encoder_vel_y) <= 2.1f
                   && fabs(desiredPos.z - actualPos.z) <= 0.2f)

                   ||

                   (final_delay_cnt == 0
                   && fabs(ret_rawx-desiredPos.x) <= 5.5f
                   && fabs(ret_rawy-desiredPos.y) <= 5.5f
                   && fabs(desiredPos.z - actualPos.z) <= 0.4f)
               )
               {

                   if(path_cnt == 3)
                   {
                       car_sta = STOPPED;
                   }
                   else if(path_cnt == 7)
                   {
                       car_sta = STOP;
                   }

//              pidReset(&leftFrontPid);
//              pidReset(&rightFrontPid);
//              pidReset(&rightBackPid);
//              pidReset(&leftBackPid);

               }


               BEEP_OFF;
               if(final_delay_cnt > 0)   final_delay_cnt--; ///定时时间到 准备进入角度回正

           }


        //6.检测错误 数据长时间不更新 判断错误
       if(uwb_vel_x == 0 && uwb_vel_y == 0)
       {
           error_cnt++;
           if(error_cnt >= 15)
           {
               error_cnt = 0;
               return 1;
           }

       }
       else
       {
           error_cnt = 0;
       }

   }

//  printf("a:%.2f,%.2f,%.2f\n",actualPos.x,actualPos.y,actualPos.z);

   return 0;
}


